#include "config.h"
#include "m14kc.h"

        .set noreorder         		# Linker don't change instruction order
        .set noat               	# Linker turn on $at warnings


        .section 	.vector, "ax" 		# Linker allocatable and executable section
        .globl  	_HW_Vector     		# Linker global variable
        .ent    	_HW_Vector      	# Linker start of function
_HW_Vector:
        /* at address 0x80000180 or 0x90000180     							*/
        /* No need to save gpr registers because we are using a shadow      */
        /* register set                                                     */
        /* Save the Status and EPC registers so we can restore tthem after  */
        /* we return form c code 											*/
        /* NOTE: in this example we have only one interrupt source so this  */
        /* is not strictly necessary we do it here to setup the ability to  */
        /* take another interrupt while we are processing this one and      */
        /* because we will be changing the Status register and it will need */
        /* to be restored.                                                  */
        /* Also allows for 16 bytes for arguments (C ABI requirment)        */
        di
#ifdef CONFIG_MMU
	mfc0	t1, CP0_CAUSE
	andi	t1, t1, 0x7c
	xori	t1, t1, 0x20
	bne		t1, zero, 1f
	nop							# is syscall
	mfc0    a0, CP0_EPC			# Get the value of EPC
	ehb
	bal		syscall_hook
	nop
	bne		v0, zero, 2f		#
	nop
	mfc0    t1, CP0_STATUS		# Get the value of the status register
	ori		t1, t1, C0_STS_UM
	xori	t1, t1, C0_STS_UM	# force to KERNEL mode
	mtc0	t1, CP0_STATUS		# Write it back to C0_STATUS
2:
	mfc0    t1, CP0_EPC			# Get the value of EPC
	addiu	t1, 4				# skip to next instruction
	mtc0    t1, CP0_EPC			# Write it back to C0_EPC
	ehb
	ei
	eret
1:
#endif
        addiu   	sp, sp, -28             	# push stack for registers +args
        mfc0    	t1, CP0_STATUS          	# Get the value of the status register
        sw      	t1, 20(sp)             		# Store status value on the stack
        mfhi		t1					# Backup HI LO regsiter
        sw		t1, 12(sp)
        mflo		t1
        sw		t1, 8(sp)
        mfc0    	t2, CP0_EPC             	# Get the value of EPC
#if !defined(CONFIG_MMU)
	mfc0	t1, CP0_CAUSE
	andi	t1, t1, 0x7c
	xori	t1, t1, 0x20
	bne		t1, zero, 3f
	nop							# is syscall
	addiu	t2, 4				# skip to next instruction
#endif
3:
        sw      	t2, 24(sp)              	# Store the value of EPC on the stack

        jal 		exception_handler          	# Jump to C function
        nop

	lw		t2, 12(sp)
	mthi		t2
	lw		t2, 8(sp)
	mtlo		t2
        /* Make sure the status register EXL an IE bits are set the same as */
        /* when we jumped to C code                                         */
        lw      	t2, 24(sp)              	# retrieve the saved value of C0_EPC        
        mtc0    	t2, CP0_EPC       			# Write it back to C0_EPC
        ehb                             		# Wait for change to take effect

        lw      	t1, 20(sp)              	# retrieve the value of C0_STATUS
        mtc0    	t1, CP0_STATUS  			# Write the value to C0_STATUS
        ehb                             		# Wait for change to take effect

        addiu   	sp, sp, 28              	# set the stack pointer back to where
                                        		# it was when interrupt routine was entered
		ei
		eret                            		# Exception Return (will clear EXL)
		.end    	_HW_Vector      			# Linker end of function
		.set		at
		.set		reorder


#ifdef CONFIG_MMU
	.section	.text,  "ax"
	.set	noreorder 			# Linker don't change instruction order
	.set	noat
	.globl	user_mode	   		# Linker global variable
	.globl	api_ret2user
	.ent	user_mode  	   		# Linker start of function
api_ret2user:
	la		t1, _sp
	lw		ra, 0(t1)

user_mode:
    mtc0    ra, CP0_EPC
	mfc0    t1, CP0_STATUS		# Get the value of the status register
	ori		t1, t1, C0_STS_UM | C0_STS_EXL
	mtc0	t1, CP0_STATUS		# Write it back to C0_STATUS
	ehb
	eret
	.end    user_mode      		# Linker end of function
	.set	at
	.set	reorder

	.section	.syscall,"ax",@progbits
	.align	2
	.set	noreorder 			# Linker don't change instruction order
	.set	noat
	.globl	kernel_mode	   		# Linker global variable
	.ent	kernel_mode  	   	# Linker start of function
	.type	kernel_mode, @function
kernel_mode:
	syscall
	jrc	ra
	nop
	.end	kernel_mode
	.set	at
	.set	reorder
#endif

